﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using TracedReality;
using TracedReality.Primitive;

using OpenTK;

namespace Interface.Visualization
{
    public struct Perspective
    {
        public double fov;
        public double aspect;
        public double near;
        public double far;
    }

    class FrustumPlane
    {
        Vector point;
        Vector normal;
        double d;

        public Vector Normal { get { return normal; } }

        public FrustumPlane(Vector p, Vector n)
        {
            point = new Vector(p);
            normal = new Vector(n);
            d = -normal.innerProduct(point);
        }

        public FrustumPlane(double a, double b, double c, double d)
        {
            normal = new Vector(a, b, c);
            double l = normal.length;
            normal /= l;
            this.d = d / l;
        }

        public double distance(Vector p)
        {
            return d + normal.innerProduct(p);
        }

        public override string ToString()
        {
            return string.Format("[normal: {0}, d: {1}]", normal, d);
        }
    }

    public class ViewFrustum
    {
        private static Vector4d[] cPlanes = new Vector4d[] { new Vector4d(-1,  0,  0, 1),
                                                              new Vector4d( 1,  0,  0, 1),
                                                              new Vector4d( 0, -1,  0, 1),
                                                              new Vector4d( 0,  1,  0, 1),
                                                              new Vector4d( 0,  0, -1, 1),
                                                              new Vector4d( 0,  0,  1, 1) };

        Matrix4d pMatrix;
        Matrix4d tMatrix;
        FrustumPlane[] planes;
        Vector translation;

        public ViewFrustum(Perspective perspective)
        {
            planes = new FrustumPlane[6];

            pMatrix = Matrix4d.Perspective(perspective.fov, perspective.aspect, perspective.near, perspective.far);

            translation = new Vector();
        }

        public void SetCamera(GLCamera camera)
        {
            Matrix4d cMatrix = Matrix4d.LookAt(camera.Position, camera.Interest, camera.Up);
            Matrix4d.Mult(ref cMatrix, ref pMatrix, out tMatrix);

            Vector4d pl;
            for (int i = 0; i < cPlanes.Length; i++)
            {
                Vector4d.Transform(ref cPlanes[i], ref tMatrix, out pl);
                planes[i] = new FrustumPlane(pl.X, pl.Y, pl.Z, pl.W);
            }
        }

        public void SetTranslation(Vector t) { translation = t; }

        private Vector GetVertexP(BoundingBox bb, Vector n)
        {
            Vector v = new Vector(bb.min);
            if (n.x >= 0) { v.x = bb.max.x; }
            if (n.y >= 0) { v.y = bb.max.y; }
            if (n.z >= 0) { v.z = bb.max.z; }
            return v;
        }

        private Vector GetVertexN(BoundingBox bb, Vector n)
        {
            Vector v = new Vector(bb.max);
            if (n.x >= 0) { v.x = bb.min.x; }
            if (n.y >= 0) { v.y = bb.min.y; }
            if (n.z >= 0) { v.z = bb.min.z; }
            return v;
        }

        private Vector TransformPoint(Vector p)
        {
            double x = p.x * tMatrix.M11 + p.y * tMatrix.M21 + p.z * tMatrix.M31 + tMatrix.M41;
            double y = p.x * tMatrix.M12 + p.y * tMatrix.M22 + p.z * tMatrix.M32 + tMatrix.M42;
            double z = p.x * tMatrix.M13 + p.y * tMatrix.M23 + p.z * tMatrix.M33 + tMatrix.M43;
            return new Vector(x, y, z);
        }

        public enum IntersectionResult { Inside, Outside, Intersecting };

        public IntersectionResult PointInFrustum(Vector p)
        {
            Vector tp = TransformPoint(p);
            for (int i = 0; i < 6; i++)
            {
                if (planes[i].distance(tp) < 0) { return IntersectionResult.Outside; }
            }

            return IntersectionResult.Inside;
        }

        // http://zach.in.tu-clausthal.de/teaching/cg_literatur/lighthouse3d_view_frustum_culling/index.html
        public IntersectionResult BoundingBoxInFrustum(BoundingBox bb)
        {
            IntersectionResult result = IntersectionResult.Inside;

            for (int i = 0; i < 6; i++)
            {
                FrustumPlane pl = planes[i];
                Vector pVert = GetVertexP(bb, pl.Normal);
                Vector nVert = GetVertexN(bb, pl.Normal);
                double pDist = pl.distance(pVert + translation);
                double nDist = pl.distance(nVert + translation);

                if (pDist < 0) { return IntersectionResult.Outside; }
                else if (nDist < 0) { result = IntersectionResult.Intersecting; }
            }

            return result;
        }
    }
}
